
#include <Eigen/Dense>
#include <cassert>
#include <cmath>
#include <fstream>
#include <iomanip>
#include <iostream>
using namespace std;
using namespace Eigen;

constexpr double rho{1.23}, mu{17.8e-6}, vel{35}, ka{1.2}, e{0.96},
    pi{3.1415926535897932}, averg_thick{0.12}, n_ult{2.5}, W0{4940},
    c1{0.03062702}, c2{2.05};

struct Weight {
  const double wb{W0 * 1.3};
  double calcW(double A, double S, double tol = 1.0e-5) const {
    double w1, w2{wb};
    do {
      w1 = w2;
      w2 = W0 + 45.42 * S +
           8.71 * (1.0e-5) * n_ult * A * sqrt(A * S * W0 * w1) / averg_thick;
    } while (fabs(w2 - w1) > tol);
    return w2;
  }
};

struct Drag {
  const double rhov{rho * vel};
  const double coe1{ka * 0.074 * 2.05 / pow(rhov / mu, 0.2) * 0.5 * rhov * vel};
  const double coe2{2 / (rhov * vel * pi * e)};
  const double coe3{0.5 * rhov * vel * c1};
  double delta{1.0e-5};
  Weight totalw;

  double operator()(double S, double A, double W) const {

    return coe3 + coe1 * pow(A, 0.1) * pow(S, 0.9) + coe2 * W * W / (S * A);
  }
  double operator()(const Vector2d &xk, double W) const {
    double A{xk(0)}, S{xk(1)};
    return coe3 + coe1 * pow(A, 0.1) * pow(S, 0.9) + coe2 * W * W / (S * A);
  }

  // double partialS(double S, double A, double W) const {
  //   return 0.9 * coe1 * pow(A, 0.1) * pow(S, -0.1) - coe2 * W * W / (S * S *
  //   A);
  // }
  // double partialA(double S, double A, double W) const {
  //   return 0.1 * coe1 * pow(A, -0.9) * pow(S, 0.9) - coe2 * W * W / (S * A *
  //   A);
  // }
  double partialS(double S, double A, double W) const {
    double w2 = totalw.calcW(A, S + delta);
    double f2 = (*this)(S + delta, A, w2);
    double f = (*this)(S, A, W);
    return (f2 - f) / delta;
  }
  double partialA(double S, double A, double W) const {
    double w2 = totalw.calcW(A + delta, S);
    double f2 = (*this)(S, A + delta, w2);
    double f = (*this)(S, A, W);
    return (f2 - f) / delta;
  }
  void print() { cout << coe1 << "\n" << coe2 << endl; }
};

struct WolfeCon {
  double mu1{1.0e-10}, mu2{1};
  Drag f;
  bool operator()(Vector2d &xk, Vector2d &gk, Vector2d &pk, double alpha,
                  Vector2d &gkp1, Vector2d &xkp1, double w, double w2) {

    // if (f(xkp1, w) > f(xk, w) + mu1 * alpha * pk.transpose() * gk)
    //   return false;
    // if (abs(gkp1.transpose() * pk) > mu2 * abs(gk.transpose() * pk))
    //   return false;

    // cout << abs(gkp1.transpose() * pk) << "  fjkdlfslf  "
    //      << mu2 * abs(gk.transpose() * pk) << endl;
    // cout << "gradA = " << grad(0) << "  gradS = " << grad(1) << endl;
    if ((f(xkp1, w) <= f(xk, w2) + mu1 * alpha * pk.transpose() * gk) &&
        (fabs(gkp1.transpose() * pk) <= mu2 * fabs(gk.transpose() * pk)))
      // 弱 条件
      // if ((f(xkp1, w) <= f(xk, w) + mu1 * alpha * pk.transpose() * gk) &&
      //     (gkp1.transpose() * pk >= mu2 * gk.transpose() * pk))
      return true;
    else
      return false;
  }
};

struct Golden {
  Drag f;
  Weight totalw;
  const double g = 0.5 * (sqrt(5) - 1);
  // 求 alpha
  double operator()(const Vector2d &xk, double &a, double &b,
                    const Vector2d &pk, double w) const {
    double x1, x2;
    double f1, f2, w1, w2;
    x1 = a + (1 - g) * (b - a);
    x2 = a + g * (b - a);
    Vector2d xkp1 = xk + x1 * pk;
    Vector2d xkp2 = xk + x2 * pk;

    w1 = totalw.calcW(xkp1(0), xkp1(1));
    w2 = totalw.calcW(xkp2(0), xkp2(1));
    f1 = f(xkp1, w1);
    f2 = f(xkp2, w2);

    // if (0.5 * (b - a) < TOL || f1 == f2)
    //   break;
    if (f1 < f2) {
      b = x2;
      x2 = x1;
      f2 = f1;
      x1 = a + (1 - g) * (b - a);
      f1 = f(xk + x1 * pk, w);
    } else {
      a = x1;
      x1 = x2;
      f1 = f2;
      x2 = a + g * (b - a);
      f2 = f(xk + x2 * pk, w);
    }
    return 0.5 * (a + b);
  }
};

struct FrMethod {
  Drag drag;
  Weight totalw;
  Golden golden_search;
  WolfeCon condition;
  // 0: 对A  1：对S
  void calcGrad(Vector2d &grad, const double &A, const double &S,
                const double &W) {
    grad(0) = drag.partialA(S, A, W);
    grad(1) = drag.partialS(S, A, W);
  }
  // 返回最小值点
  Vector2d LineSearch(double A, double S, const double grad_tol = 1.0e-12,
                      const int max_step = 100000, double tol = 1.0e-8) {
    Vector2d grad, pk, pkp1, gradkm1, xk, xkp1, gradkp1;
    double W, mbeta, tmp, W2;
    int step{0};
    bool tag{false};

    double alpha, min_alpha, max_alpha, alpha2;
    xk(0) = A;
    xk(1) = S;
    do {
      step++;
      W = totalw.calcW(xk(0), xk(1));
      grad(0) = drag.partialA(xk(1), xk(0), W);
      grad(1) = drag.partialS(xk(1), xk(0), W);

      tag = false;

      min_alpha = 1.0e-15;
      max_alpha = 4.0;

      if (sqrt(grad(0) * grad(0) + grad(1) * grad(1)) < grad_tol)
        break;
      // 不归一话了
      if (step == 1) {
        pkp1 = -1.0 * grad;

      } else {
        mbeta = (grad(0) * grad(0) + grad(1) * grad(1)) /
                (gradkm1(0) * gradkm1(0) + gradkm1(1) * gradkm1(1));
        // mbeta = 0.0;
        pkp1 = -1.0 * grad + mbeta * pk;
      }
      gradkm1 = grad;
      pk = pkp1;

      if (pkp1(0) != 0)
        max_alpha = fabs(xk(0) / pkp1(0));
      if (pkp1(1) != 0)
        max_alpha = min(fabs(xk(1) / pkp1(1)), max_alpha);

      cout << "\ngradA = " << grad(0) << "  gradS = " << grad(1) << "\n";
      cout << "A = " << xk(0) 
           << "  S = " << xk(1) << "\n";
      cout << "pkp1 = " << pkp1(0) << "  " << pkp1(1) << endl;
      //   break;
      // 一维搜索确定 步长
      // int substep{0};
      // double delta_alpha{100};
      alpha = -100.0;
      alpha2 = -1;
      while (tag != true && (fabs(alpha - alpha2) > 1.0e-10)) {
        alpha2 = alpha;
        alpha = golden_search(xk, min_alpha, max_alpha, pkp1, W);
        xkp1 = xk + alpha * pkp1;
        W2 = W;
        W = totalw.calcW(xkp1(0), xkp1(1));
        gradkp1(0) = drag.partialA(xkp1(1), xkp1(0), W);
        gradkp1(1) = drag.partialS(xkp1(1), xkp1(0), W);
        // cout << "sA = " << xkp1(0) << "\n"
        //      << "sS = " << xkp1(1) << "alpha = " << alpha << endl;

        if (sqrt(gradkp1(0) * gradkp1(0) + gradkp1(1) * gradkp1(1)) < grad_tol)
          break;
        // cout << "alpha = " << alpha << endl;
        tag = condition(xk, grad, pkp1, alpha, gradkp1, xkp1, W, W2);
        // alpha *= 0.5;
        // substep++;
        // if (substep == 10)
        //   break;
      }
      //   cout << "alpha = " << alpha << endl;
      // if (tag != true)
      //   alpha = 1.0e-14;
      xk += alpha * pkp1;
      tmp = alpha * sqrt(pkp1(0) * pkp1(0) + pkp1(1) * pkp1(1));
      cout << "step = " << step;
      // if (step == 3)
      //   break;
      // break;
    } while (tmp > tol && step < max_step);
    // cout << "total weight = " << W << endl;
    return xk;
  }
};

int main() {
  // Weight w;
  // cout << w.calcW(20, 20.8) << endl;
  // Drag d;
  //   d.print();
  Drag d;
  Weight w;
  cout << setprecision(10) << scientific;
  FrMethod min_drag;
  // Vector2d solution = min_drag.LineSearch(14.2764, 11.6743);
  Vector2d solution = min_drag.LineSearch(3, 2);
  double Wght = w.calcW(solution(0), solution(1));
  cout << "\nresult: \n";
  cout << "A = " << solution(0) 
       << "  S = " << solution(1) << "  D = " << d(solution, Wght) << endl;

  // ofstream osf{"cd.plt"};

  // A = 20.8283
  // S = 17.698
  // double dx = 0.5, s = 17.698, a = 20.8283;
  // // osf << "  variables = \"a\", \"b\", \"c\", \"d\", \"e\"\n";
  // // osf << "ZONE I = 1000, J = 1000\n";
  // double min_grad{100000000}, max{-1.0}, pa, pb;
  // // for (int i = 1; i < 1001; ++i)
  // //   for (int j = 1; j < 1001; ++j) {
  // //     s = i * dx;
  // //     a = j * dx;
  // double www = w.calcW(a, s);
  // //     osf << s << "  " << a << "  ";
  // pa = d.partialA(s, a, www);
  // pb = d.partialS(s, a, www);
  // //     osf << d(s, a, www) << " " << pa << " "
  // //         << pb << "\n";

  // //     // min_grad = (abs(pa*pa + pb*pb) < )
  // //   }

  // cout << pa << "  " << pb << "  \n";
  return 0;
}